#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>

static int u(int BitCount,const unsigned char * buf,int &offset)
{
    int dwRet = 0;
    for (int i=0; i<BitCount; ++i)
    {
        dwRet <<= 1;
		if (buf[offset / 8] & (0x80 >> (offset % 8)))
		//if ((0x80 >> (offset % 8)) == (buf[offset / 8] & (0x80 >> (offset % 8))))
        {
            dwRet += 1;
        }
        ++offset;
    }
    return dwRet;
}

static int GetGolombValue(const unsigned char *s, int offset, unsigned *value)
{
    /*printf("GetGolombValue 0x%08x+%02d value@0x%08x %s", (int32_t)s, offset, (int32_t)value, (value)?"-->":"\n");*/
    int len = -1;
    for (int b = 0; b == 0; ++len, ++offset) {
        if (offset >= 8) {
            s += offset >> 3;
            offset %= 8;
        }
        b = ((*s) >> (7 - offset)) & 0x1;
    }

    if (value) {
        *value = (1 << len) - 1;
        unsigned t = 0;
        for (int i = 0; i < len; ++i, ++offset) {
            if (offset >= 8) {
                s += offset >> 3;
                offset %= 8;
            }
            t <<= 1;
            t += ((*s) >> (7 - offset)) & 0x1;
        }
        *value += t;
        /*printf("%d\n", *value);*/
    }

    return len * 2 + 1;
}
static int GetUeGolombValue(const unsigned char *s, int offset, unsigned *value)
{
	//计算0bit的个数
	int nZeroNum = 0;
	int iStartBit = offset;
	const uint8_t *bData = s;
	while (iStartBit < 40*8)
	{
		if ((0x80 >> (iStartBit % 8)) == (bData[iStartBit / 8] & (0x80 >> (iStartBit % 8)))) //&:按位与，%取余
		{
			break;
		}
		nZeroNum++;
		iStartBit++;
	}
	nZeroNum = nZeroNum + 1;
	//计算结果
	int dwRet = 0;
	for (int i = 0; i < nZeroNum; i++)
	{
		dwRet <<= 1;
		if ((0x80 >> (iStartBit % 8)) == (bData[iStartBit / 8] & (0x80 >> (iStartBit % 8))))
		{
			dwRet += 1;
		}
		iStartBit++;
	}
	if(value) *value = dwRet-1;
	return iStartBit - offset;
}

static int GetSeGolombValue(const unsigned char *s, int offset, unsigned *value)
{
	//计算0bit的个数
	int nZeroNum = 0;
	int old_offset = offset;
	while (offset < 40* 8)
	{
		if ((0x80 >> (offset % 8)) == (s[offset / 8] & (0x80 >> (offset % 8)))) //&:按位与，%取余
		{
			break;
		}
		nZeroNum++;
		offset++;
	}
	//计算结果
	int dwRet = 0;
	for (int i = 0; i < nZeroNum; i++)
	{
		dwRet <<= 1;
		if ((0x80 >> (offset % 8)) == (s[offset / 8] & (0x80 >> (offset % 8))))
		{
			dwRet += 1;
		}
		offset++;
	}
	if ((0x80 >> (offset % 8)) == (s[offset / 8] & (0x80 >> (offset % 8))))
	{
		dwRet = 0-dwRet;
	}
	offset++;
	if(value) *value = dwRet;
	return offset - old_offset;
}

static int32_t parseSPS(const unsigned char *sps, int *width, int *height)
{
    unsigned v;
    int off = 0;

    uint8_t profile_idc = *((uint8_t *)sps);
	printf("=== profile_idc = %d\n",profile_idc);
    sps += 3;

    off += GetGolombValue(sps, off, NULL);	//seq_parameter_set_id

    unsigned chroma_format_idc = 1;
    if (profile_idc == 100 || profile_idc == 110 ||
	    profile_idc == 122 || profile_idc == 244 || profile_idc == 44 ||
	    profile_idc == 83  || profile_idc == 86  || profile_idc == 118)
	{
		off += GetGolombValue(sps, off, &chroma_format_idc); //chroma_format_idc
		//printf("----chroma_idc = %d\n",chroma_format_idc);
		if (chroma_format_idc == 3)
		{
			off += 1; //residual_colour_transform_flag
		}
		unsigned bit_depth_luma_minus8,bit_depth_chroma_minus8;
		off += GetGolombValue(sps, off, &bit_depth_luma_minus8); //bit_depth_luma_minus8
		off += GetGolombValue(sps, off, &bit_depth_chroma_minus8); //bit_depth_chroma_minus8
		//printf("xxx bit_depth_luma_minus8 = %d,bit_depth_chroma_minus8 = %d\n",bit_depth_luma_minus8,bit_depth_chroma_minus8);
		//off += 1; //qpprime_y_zero_transform_bypass_flag
		int qpprime_y_zero_transform_bypass_flag = u(1,sps,off);
		//printf("xxx qpprime_y_zero_transform_bypass_flag = %d\n ",qpprime_y_zero_transform_bypass_flag);
		uint16_t seq_scaling_matrix_present_flag = u(1, sps, off); //seq_scaling_matrix_present_flag
		//printf("   seq_scaling_matrix_present_flag = %d\n ",seq_scaling_matrix_present_flag);
		if (seq_scaling_matrix_present_flag)
		{
			int seq_scaling_list_present_flag[8] = {0};
			for(int i = 0; i < 8; i++ )
			{
				//printf("xxx off = %d\n",off);
				seq_scaling_list_present_flag[i] = u(1, sps, off);
				//printf("xxx seq_scaling_list_present_flag[%d] = %d\n",i,seq_scaling_list_present_flag[i]);
				if( seq_scaling_list_present_flag[i] )
				//if(0)
				{
					if (i<6)
					{
						int lastScale = 8 ;
						int nextScale = 8 ;
						int	delta_scale;
						int	scalingListEx[64] = {0};
						for(int j = 0; j < 16; j++ ) {
							if( nextScale != 0 ) {
								//off += GetGolombValue(sps, off, (unsigned*)&delta_scale);
								off += GetSeGolombValue(sps, off, (unsigned*)&delta_scale);
								nextScale = ( lastScale + delta_scale + 256 ) % 256 ;
							}
							if(!j && !nextScale) {
								break;
							}
							scalingListEx[ j ] = ( nextScale  ==  0 ) ? lastScale : nextScale ;
							lastScale = scalingListEx[ j ] ;
						}
					}
					else
					{
						int lastScale = 8 ;
						int nextScale = 8 ;
						int	delta_scale;
						int	scalingListEx[64] = {0};
						for(int j = 0; j < 64; j++ ) {
							if( nextScale != 0 ) {
								//off += GetGolombValue(sps, off, (unsigned*)&delta_scale);
								off += GetSeGolombValue(sps, off, (unsigned*)&delta_scale);
								nextScale = ( lastScale + delta_scale + 256 ) % 256 ;
							}
							if(!j && !nextScale) {
								break;
							}
							scalingListEx[ j ] = ( nextScale  ==  0 ) ? lastScale : nextScale ;
							lastScale = scalingListEx[ j ] ;
						}
					}
				}
			}
			/*for (int i=0; i<8; i++)
			  {
			  off += 1;//seq_scaling_list_present_flag[i]
			  }*/
		}
	}

	unsigned log2_max_frame_num_minus4 = 0;
    off += GetGolombValue(sps, off, &log2_max_frame_num_minus4);	//log2_max_frame_num_minus4
	//printf("xxx log2_max_frame_num_minus4 = %d\n",log2_max_frame_num_minus4);
    off += GetGolombValue(sps, off, &v);	//pic_order_cnt_type
	//printf(" ---pic_order_cnt_type  = %d\n " ,v);
    if (v == 0)
    {
		off += GetGolombValue(sps, off, NULL);	//log2_max_pic_order_cnt_lsb_minus4
    }
    else if (v == 1)
	{
		off += 1;//delta_pic_order_always_zero_flag
		off += GetSeGolombValue(sps, off, NULL); // offset_for_non_ref_pic
		off += GetSeGolombValue(sps, off, NULL); //offset_for_top_to_bottom_field
		unsigned cnt = 0;
		off += GetGolombValue(sps, off, &cnt); //num_ref_frames_inpic_order_cnt_cycle
		if (cnt > 256)
		{
			cnt = 256;
		}
		for(unsigned i=0;i<cnt;i++)
			off += GetSeGolombValue(sps, off, NULL);/* 用来解码poc，对于循环中的每个元素指定一个偏移 */
	}
    else if (v > 2)
    {
		//白皮书说没有这种状况
    }


    off += GetGolombValue(sps, off, &v);	//num_ref_frames
    off += 1;	//graps_in_num_value_allowed_flag

    unsigned vW = 0, vH=0, crop_left=0, crop_right=0, crop_top=0, crop_bottom=0;
    off += GetGolombValue(sps, off, &vW);	//pic_width_in_mbs_minus1
    off += GetGolombValue(sps, off, &vH);	//pic_heigth_in_map_units_minus1

    unsigned frame_mbs_only_flag = u(1, sps, off);//frame_mbs_only_flag
    if (!frame_mbs_only_flag)
	{
		off += 1;//mb_adaptiv_frame_field_flag
	}
	off += 1;//direct_8x8_inference_flag
	uint16_t frame_cropping_flag = u(1, sps, off);//frame_cropping_flag
	if (frame_cropping_flag)
	{
		off += GetGolombValue(sps, off, &crop_left);//frame_crop_left_offset
		off += GetGolombValue(sps, off, &crop_right);//frame_crop_right_offset
		off += GetGolombValue(sps, off, &crop_top);//frame_crop_top_offset
		off += GetGolombValue(sps, off, &crop_bottom);//frame_crop_bottom_offset
    }

	printf("-- frame_mbs_only_flag = %d,frame_cropping_flag = %d,vW = %d,vH = %d,crop_left = %d,crop_right = %d,top = %d,bottom = %d\n",
			frame_mbs_only_flag,frame_cropping_flag,vW,vH,crop_left,crop_right,crop_top,crop_bottom);
    //*width = ((vW+1)*16) - crop_left*2 - crop_right*2;
    //*height= ((2 - frame_mbs_only_flag) * (vH+1) * 16) - (crop_top * 2) - (crop_bottom * 2);
	*width = (vW+1)*16;
	*height = (2 - frame_mbs_only_flag)*(vH+1)*16;

	if (1 == frame_cropping_flag)
	{
		int crop_unit_x;
		int crop_unit_y;
		if (0 == chroma_format_idc) // monochrome
		{
			crop_unit_x = 1;
			crop_unit_y = 2 - frame_mbs_only_flag;
		}
		else if (1 == chroma_format_idc) // 4:2:0
		{
			crop_unit_x = 2;
			crop_unit_y = 2 * (2 - frame_mbs_only_flag);
		}
		else if (2 == chroma_format_idc) // 4:2:2
		{
			crop_unit_x = 2;
			crop_unit_y = 2 - frame_mbs_only_flag;
		}
		else // 3 == sps.chroma_format_idc   // 4:4:4
		{
			crop_unit_x = 1;
			crop_unit_y = 2 - frame_mbs_only_flag;
		}

		*width -= crop_unit_x * (crop_left+crop_right);
		*height -= crop_unit_y * (crop_top+crop_bottom);
	}
    return 0;
}

static int32_t parseH264NaluSPS(const unsigned char *pH264Data, int *pWidth, int *pHeight)
{
    /*Verify SPS nalu signature 0x00 0x00 0x00 0x01 0x67*/
    if (*((uint32_t*)pH264Data) != 0x01000000)
    {
        return -1;
    }
    #define SPS_NALU    (0x67)
    if (*(pH264Data + 4) != SPS_NALU)
    {
        return -1;
    }
    return parseSPS(pH264Data + 5, pWidth, pHeight);
}

int32_t GetResolution(const unsigned char *pSPS, int *pWidth, int *pHeight)
{
    *pWidth     = 0;
    *pHeight    = 0;
    return parseSPS(pSPS, pWidth, pHeight);
}


int main(int argc,char *argv[])
{
	//Hight Profile
	const uint8_t hsps[] = {0x64,0x00,0x2a,0xad,0x84,0x01,0x0c,0x20,0x08,0x61,0x00,0x43,0x08,0x02,0x18,0x40,0x10,0xc2,0x00,0x84,0x3b,0x50,0x3c,0x01,0x13,0xf2,0xcd,
		0xc0,0x40,0x40,0x50,0x00,0x00,0x3e,0x80,0x00,0x0c,0x35,0x08,0x40,0x00};
	//Main Profile
	const uint8_t msps[] = {0x4D,0x00,0x2A,0x9D,0xA8,0x1E,0x00,0x89,0xF9,0x66,0xE0,0x20,0x20,0x28,0x00,0x00,0x1F,0x40,0x00,0x06,0x1A,0x84,0x20,0x00,0xC1,0xF6,0xC0};
	int w = 0,h = 0;

	int ret = GetResolution(hsps,&w,&h);
	printf("====High Profile SPS len = %zu, width = %d,hight = %d ====\n",sizeof(hsps),w,h);

	ret = GetResolution(msps,&w,&h);
	printf("====Main Profile SPS len = %zu,width = %d,hight = %d ====\n",sizeof(msps),w,h);
	return ret;
}

